global bound l result npout
l=[1,1,1,0.5];
bound=[ -pi     pi;% qm1
        -pi     pi;% qm2
        -pi     pi;% qm3
        -pi     pi;% qm4
       -pi/4   pi/4;% vqm1
       -pi/4   pi/4;% vqm2
       -pi/4   pi/4;% vqm3
       -pi/4   pi/4;% vqm4
        -pi     pi;% phis1
        -pi     pi;% phis2
        -pi     pi;% phif1
        -pi     pi;% phif2
        0       8;% t1 
        0       8];%t2
npout=40;
% hh=findobj(gcf,'tag','x1');
% xs=str2num(get(hh,'string'));
% hh=findobj(gcf,'tag','y1');
% ys=str2num(get(hh,'string'));
% hh=findobj(gcf,'tag','phi');
% phis=str2num(get(hh,'string'));
% hh=findobj(gcf,'tag','x2');
% xf=str2num(get(hh,'string'));
% hh=findobj(gcf,'tag','y2');
% yf=str2num(get(hh,'string'));
xs=1;ys=0;
xf=1;yf=3;

% qs=invkin4(xs,ys,xs2,ys2);
% qqs=forkin4(qs(1),qs(2),qs(3),qs(4));
% qf=invkin4(xf,yf,xf2,yf2);
% qqf=forkin4(qf(1),qf(2),qf(3),qf(4));
para=[xs ys xf yf];

% cacu_result(4,para,100,120,@fitnessf);
% [x1,x2,x4]=trajectory_planningf_GA(para,100,200,@fitnessf,@trajplot4,@paraplot4);
% [x1,x2,x4]=trajectory_planningf_PSO(para,100,200,@fitnessf,@trajplot4,@paraplot4)
[x1,x2,x4]=trajectory_planningf_BSO(para,100,200,@fitnessf,@trajplot4,@paraplot4)
% [x21,x22,x23,x24]=trajectory_planningf_xBSO(para,@recor4,@trajplot4,@paraplot4)
% [x1,x2,x4]=trajectory_planningf_BAS(para,1000,@fitnessf,@trajplot4,@paraplot4);

function aa = fitnessf(pop,para)
global l npout
[pops,~]=size(pop);
for i=1:pops
    fit=0;
    a_v=[ 2;  %ft 2
          0;  %fq 1.8
          0;  %fdis 2
          1]; %time 1
    bsol=pop(i,:);
    time=bsol(13)+bsol(14);
    [m_s,confs]=invkin4(para(1),para(2),bsol(9),bsol(10),l);
    [m_f,conff]=invkin4(para(3),para(4),bsol(11),bsol(12),l);
%     bsol(9:10)=phis;
%     bsol(11:12)=phif;
    if strcmp(m_s,'no-redundancy') || strcmp(m_f,'no-redundancy')
        if strcmp(m_s,'no-redundancy')
            fit=fit+(confs(1))*100;
        end
        if strcmp(m_f,'no-redundancy')
            fit=fit+(conff(1))*100;
        end
        aa(i)=1/fit;
        continue;
    end
    cond=[confs,conff];
    chrom=[bsol(1:8),bsol(13:14)];
    kk=trajt(cond,chrom,npout);
%     tt=torque4(kk);
%     ft=ftorque4(tt);
    ft=ftorque4(kk(9:12,:));
    poo=kk(1:4,:);
    fq=sum(sum(abs(diff(poo'))));
%     pos=forkin(poo,l);xx=pos(1,:);yy=pos(2,:);
%     x=diff(xx);
%     y=diff(yy);
%     dis=sqrt(x.^2+y.^2);
%     fdis=sum(dis);
    fdis=0;
    fit_v=[ft,fq,fdis,time];
    fit=fit_v*a_v;
    aa(i)=1/fit;
end
end

function trajplot4(para,bsol,fit,i,n)
global l kk npout
    [~,confs]=invkin4(para(1),para(2),bsol(9),bsol(10),l);
    [~,conff]=invkin4(para(3),para(4),bsol(11),bsol(12),l);
    qs=confs;
    qm=bsol(1:4);
    qg=conff;
    
    [xt,yt]=angls2links([qs' qm' qg'],l);
    cond=[confs,conff];
    chrom=[bsol(1:8),bsol(13:14)];
    kk=trajt(cond,chrom,npout);
    pcart=forkin(kk(1:4,:),l);
    px=pcart(1,:);py=pcart(2,:);
    plot(px,py)
    hold on
    axis([-5 5 -5 5])
    text(1.75,3.0,['gen. No.',num2str(i)]);
    text(1.75,2.6,['fit=',num2str(fit)]);
    plot(xt',yt')
    hold on
    plot(0,0,'ko')
    for j=1:3
        plot(xt(:,size(xt,2)/4*j),yt(:,size(xt,2)/4*j),'bo')
    end
    hold off
end

function paraplot4(n,maxf,bsol,para)
global l kk
%     trec=rec(1,:);
%     qrec=rec(2,:);
%     drec=rec(3,:);
%     torrec=rec(4,:);
    figure,manimove_anime(kk(1:4,:),l);

    e=[1:n];
    figure,plot(e,1./maxf)
    xlabel('generation')
    ylabel('min. fitness')
end
